#include "xtypes.h"
#include "util_matrix.h"
#include "xmath.h"
#include "util_point.h"
#include <stdlib.h>
#include <string.h>


point3_t xPt3MulMat4x4(point3_t pt3, matrix4x4_t m44)
{
	point4_t pt4;
	pt4 = xGenPoint4(pt3);
	pt4 = xPt4MulMat4x4(pt4, m44);
	pt3 = xGenPoint3(pt4);
	return pt3;	
}

point4_t xPt4MulMat4x4(point4_t pt4, matrix4x4_t m44)
{
	point4_t t;	
	t.x = pt4.x*m44._00 + pt4.y*m44._10 + pt4.z*m44._20 + pt4.w*m44._30;
	t.y = pt4.x*m44._01 + pt4.y*m44._11 + pt4.z*m44._21 + pt4.w*m44._31;
	t.z = pt4.x*m44._02 + pt4.y*m44._12 + pt4.z*m44._22 + pt4.w*m44._32;
	t.w = pt4.x*m44._03 + pt4.y*m44._13 + pt4.z*m44._23 + pt4.w*m44._33;
	return t;
}

matrix4x4_t xMatrix4x4Add(matrix4x4_t L, matrix4x4_t R)
{
	matrix4x4_t mt;	
	int_t i;
	for(i=0;i<=15;++i)
	{
		mt.elem[i] = L.elem[i] + R.elem[i];
	}	
	return mt;
}

matrix4x4_t xMatrix4x4Sub(matrix4x4_t L, matrix4x4_t R)
{
	matrix4x4_t mt;	
	int_t i;
	for(i=0;i<=15;++i)
	{
		mt.elem[i] = L.elem[i] - R.elem[i];
	}	
	return mt;
}

matrix4x4_t xMatrix4x4Mul(matrix4x4_t L, matrix4x4_t R)
{
	matrix4x4_t mt;

	mt._00 = L._00*R._00 + L._01*R._10 + L._02*R._20 + L._03*R._30;
	mt._01 = L._00*R._01 + L._01*R._11 + L._02*R._21 + L._03*R._31;
	mt._02 = L._00*R._02 + L._01*R._12 + L._02*R._22 + L._03*R._32;
	mt._03 = L._00*R._03 + L._01*R._13 + L._02*R._23 + L._03*R._33;
	mt._10 = L._10*R._00 + L._11*R._10 + L._12*R._20 + L._13*R._30;
	mt._11 = L._10*R._01 + L._11*R._11 + L._12*R._21 + L._13*R._31;
	mt._12 = L._10*R._02 + L._11*R._12 + L._12*R._22 + L._13*R._32;
	mt._13 = L._10*R._03 + L._11*R._13 + L._12*R._23 + L._13*R._33;

	mt._20 = L._20*R._00 + L._21*R._10 + L._22*R._20 + L._23*R._30;
	mt._21 = L._20*R._01 + L._21*R._11 + L._22*R._21 + L._23*R._31;
	mt._22 = L._20*R._02 + L._21*R._12 + L._22*R._22 + L._23*R._32;
	mt._23 = L._20*R._03 + L._21*R._13 + L._22*R._23 + L._23*R._33;
	mt._30 = L._30*R._00 + L._31*R._10 + L._32*R._20 + L._33*R._30;
	mt._31 = L._30*R._01 + L._31*R._11 + L._32*R._21 + L._33*R._31;
	mt._32 = L._30*R._02 + L._31*R._12 + L._32*R._22 + L._33*R._32;
	mt._33 = L._30*R._03 + L._31*R._13 + L._32*R._23 + L._33*R._33;

	return mt;

	/*
	matrix4x4_t mt;	
	int_t i,j,k;	//i:row,j:column,k:iterator
	for(i=0;i<=3;++i)
	{
	for(j=0;j<=3;j++)
	{
	mt.elem[i*4+j] = 0.0;
	for(k=0;k<=3;k++)
	{
	mt.elem[i*4+j] += L.elem[i*4+k] * R.elem[k*4+j];
	}			
	}
	}	
	return mt;
	//*/
}

matrix4x4_t xMatrix4x4Scale(matrix4x4_t matrix, float_t s)
{
	matrix4x4_t mt;	
	int_t i;
	for(i=0;i<=15;++i)
	{
		mt.elem[i] = matrix.elem[i] * s;
	}	
	return mt;
}


matrix4x4_t xMatrix4x4Transpose(matrix4x4_t matrix)
{
	matrix4x4_t mt;	
	int_t i,j;
	for(i=0;i<=3;++i)
	{
		for(j=0;j<=3;j++)
		{
			mt.elem[i*4+j] = matrix.elem[j*4+i];
		}
	}	
	return mt;
}


CMatrix4x4::CMatrix4x4()
{
}

CMatrix4x4::CMatrix4x4(matrix4x4_t& mat)
{
	memcpy(this, &mat, sizeof(matrix4x4_t));
}


CMatrix4x4::~CMatrix4x4()
{
}


void CMatrix4x4::SetMatrix(int_t row, int_t column, float_t elem)
{
	this->elem[row*4+column] = elem;
}

void CMatrix4x4::SetMatrix(int_t series, float_t elem)
{
	this->elem[series] = elem;
}

float_t& CMatrix4x4::fnMatrix(int_t row, int_t column)
{
	return this->elem[row*4+column];
}

float_t& CMatrix4x4::fnMatrix(int_t series)
{
	return this->elem[series];
}

void CMatrix4x4::LoadZeros()
{
	int_t i;
	for(i=0;i<=15;++i)
	{
		this->fnMatrix(i) = 0;
	}
}


void CMatrix4x4::LoadIdentity()
{
	this->LoadZeros();
	int_t i;
	for(i=0;i<=3;++i)
	{
		fnMatrix(i,i) = 1;
	}
}


CMatrix4x4 CMatrix4x4::operator+(CMatrix4x4 R)
{
    matrix4x4_t m44;
    m44 = xMatrix4x4Add(*this, R);
	return m44;
}

CMatrix4x4 CMatrix4x4::operator-(CMatrix4x4 R)
{
    matrix4x4_t m44;
    m44 = xMatrix4x4Sub(*this, R);
	return m44;
}

CMatrix4x4 CMatrix4x4::operator*(float_t R)
{
    matrix4x4_t m44;
    m44 = xMatrix4x4Scale(*this, R);
	return m44;
}

CMatrix4x4 CMatrix4x4::operator*(CMatrix4x4 R)
{
    matrix4x4_t m44;
    m44 = xMatrix4x4Mul(*this, R);
	return m44;
}


CMatrix4x4 CMatrix4x4::operator~()
{
    matrix4x4_t m44;
    m44 = xMatrix4x4Transpose(*this);
	return m44;
}


